////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "BoundingVolumes.h"

#include <SmegCore/Math/Math.hpp>
#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace math {
        
////////////////////////////////////////////////////////////
/// Static
////////////////////////////////////////////////////////////
bool BoundingVolume::Overlaps( const BoundingBox& _vA, const Circle& _vB ) {
    const sf::Vector2f& vAHalfSize( _vA.GetHalfSize() );
    sf::Vector2f vACenter( _vA.GetCenter() ),
        vBCenter( _vB.GetCenter() );

    if ( !_vA.IsAxisAligned() ) {
        // Put the circle position to the box local frame
        vBCenter -= vACenter;
        math::Zero( vACenter );
        const sf::Vector2f* pAxis = _vA.GetAxis();

        vBCenter = math::VectorToLocalSpace(    vBCenter,
                                                pAxis[BoundingBox::eXAxis],
                                                pAxis[BoundingBox::eYAxis] );
    }

    float fRadius = _vB.GetRadius(),
        fHalfXRad = vAHalfSize.x + fRadius,
        fHalfYRad = vAHalfSize.y + fRadius,
        fCenterX = abs(vACenter.x - vBCenter.x),
        fCenterY = abs(vACenter.y - vBCenter.y);

    // Circle is just too far away on X or Y
    if (fCenterX > fHalfXRad ||
        fCenterY > fHalfYRad ) {
        return false;
    }

    // Circle center is inside the rectangle with the circle radius but without the corner
    if ((fCenterX <= fHalfXRad && fCenterY < vAHalfSize.y) || 
        (fCenterX < vAHalfSize.x && fCenterY <= fHalfYRad)) {
        return true;
    }

    float fCornerDistX = (fCenterX - vAHalfSize.x),
        fCornerDistY = (fCenterY - vAHalfSize.y);
    
    fCornerDistX *= fCornerDistX;
    fCornerDistY *= fCornerDistY;

    // Corner intersections
    return (fCornerDistX + fCornerDistY <= fRadius * fRadius);
}

bool BoundingVolume::Overlaps( const BoundingBox& _vA, const Line& _vB ) {
    // This method is based on the Cohen-Sutherland algorithm
    // http://en.wikipedia.org/wiki/Cohen%E2%80%93Sutherland_algorithm
    const sf::Vector2f* _pLineExtents = _vB.GetExtents();
    int iCode0 = _vA.ComputeOutCode( _pLineExtents[0].x, _pLineExtents[0].y ),
        iCode1 = _vA.ComputeOutCode( _pLineExtents[1].x, _pLineExtents[1].y ),
        iCodesOR = iCode0 | iCode1,
        iCodesAND = iCode0 & iCode1;

    if ( !iCodesOR ) {
        // Both points are INSIDE so line is inside
        // So Line overlaps
        return true;
    } else if ( iCodesAND ) {
        // Both points are :
        // - all LEFT from BoundingBox
        // - all RIGHT from BoundingBox
        // - all TOP from BoundingBox
        // - all BOTTOM from BoundingBox
        //
        // SO
        // Line doesn't overlap
        return false;
    } else if ( ( !iCode0 || !iCode1 ) ||
        iCodesOR == (RIGHT | LEFT) ||
        iCodesOR == (TOP | BOTTOM) ) {
        // One point is inside and the other outside
        // OR
        // One point is TOP and the other BOTTOM, the line go through verticaly
        // OR
        // One point is LEFT and the other RIGHT, the line go through verticaly
        //
        // SO
        // Line obviously overlaps
        return true;
    } else if ( iCode0 == LEFT || iCode1 == LEFT ) {
        // If one point is on the LEFT, test only intersection with LEFT line of the BoundingBox
        return _vB.Overlaps( _vA.GetLeftLine() );
    } else if ( iCode0 == RIGHT || iCode1 == RIGHT ) {
        // If one point is on the RIGHT, test only intersection with RIGHT line of the BoundingBox
        return _vB.Overlaps( _vA.GetRightLine() );
    } else if ( iCode0 == TOP || iCode1 == TOP ) {
        // If one point is on the TOP, test only intersection with TOP line of the BoundingBox
        return _vB.Overlaps( _vA.GetTopLine() );
    } else if ( iCode0 == BOTTOM || iCode1 == BOTTOM ) {
        // If one point is on the BOTTOM, test only intersection with BOTTOM line of the BoundingBox
        return _vB.Overlaps( _vA.GetBottomLine() );
    } else {
        // Both points are at the extremities
        // Test 2 lines connected to the corner of the BoundingBox where one of the point is
        //
        // ie:
        // iCode0 is Top-Left, test TopLine and Left Line
        // iCode0 is Bottom-Right, test BottomLine and Right Line
        //
        // Don't need to try iCode1 because if the line overlaps
        // it will go through one of the 2 line tested
        if ( (iCode0 & LEFT) && _vB.Overlaps( _vA.GetLeftLine() ) ) {
            return true;
        } else if ( (iCode0 & RIGHT) && _vB.Overlaps( _vA.GetRightLine() ) ) {
            return true;
        } else if ( (iCode0 & TOP) && _vB.Overlaps( _vA.GetTopLine() ) ) {
            return true;
        } else if ( (iCode0 & BOTTOM) && _vB.Overlaps( _vA.GetBottomLine() ) ) {
            return true;
        } else {
            return false;
        }
    }
}

bool BoundingVolume::Overlaps( const Circle& _vA, const Line& _vB ) {
    // Find the location in local space of the entity
    const sf::Vector2f* pLineExtents = _vB.GetExtents();
    sf::Vector2f LineHeading = math::NormalizedCopy( pLineExtents[1] - pLineExtents[0] ),
        LineSide = math::PerpCopy( LineHeading ),
        LocalPos = math::PointToLocalSpace( _vA.GetCenter(),
                                            LineHeading,
                                            LineSide,
                                            pLineExtents[0] );
    if ( LocalPos.x < 0.0f ) {
        return math::SquaredLength( LocalPos ) <= _vA.GetRadius() * _vA.GetRadius();
    } else {
        float fLength = math::Length( _vB.GetSize() );
        if ( LocalPos.x > fLength ) {
            return math::SquaredLength( LocalPos - sf::Vector2f( fLength, 0.0f ) ) <= _vA.GetRadius() * _vA.GetRadius();
        } else {
            return abs( LocalPos.y ) <= _vA.GetRadius();
        }
    }
}

////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
BoundingVolume::BoundingVolume( eBoundingVolume _eType, const sf::Vector2f& _Center, const sf::Vector2f& _Size ) :
    m_Center( _Center ),
    m_Size( _Size ),
    m_HalfSize( _Size * 0.5f ),
    m_eType( _eType )
{
}
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
BoundingVolume::BoundingVolume( const BoundingVolume& _Copy ) :
    m_Center( _Copy.m_Center ),
    m_Size( _Copy.m_Size ),
    m_HalfSize( _Copy.m_HalfSize ),
    m_eType( _Copy.m_eType )
{
}
    

////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
BoundingVolume::~BoundingVolume()
{
}

////////////////////////////////////////////////////////////
/// Setup the Bounding Volume
////////////////////////////////////////////////////////////
void BoundingVolume::Setup( const sf::Vector2f& _Center, const sf::Vector2f& _Size ) {
    m_Center = _Center;
    m_Size = _Size;
    m_HalfSize = _Size * 0.5f;
}

////////////////////////////////////////////////////////////
/// Does the Bounding Volume overlaps with another BoundingVolume
////////////////////////////////////////////////////////////
bool BoundingVolume::OverlapsUnknownBBPtr( const BoundingVolume* _pOther ) const {
    // BoundingBox
    const BoundingBox* pBB = dynamic_cast<const BoundingBox*>( _pOther );

    if ( pBB ) {
        return Overlaps( *pBB );
    }
    
    // Circle
    const Circle* pCircle = dynamic_cast<const Circle*>( _pOther );

    if ( pCircle ) {
        return Overlaps( *pCircle );
    }
    
    // Line
    const Line* pLine = dynamic_cast<const Line*>( _pOther );

    if ( pLine ) {
        return Overlaps( *pLine );
    }

    return false;
}

////////////////////////////////////////////////////////////
/// Does the Bounding Volume contains another BoundingVolume
////////////////////////////////////////////////////////////
bool BoundingVolume::ContainsUnknownBBPtr( const BoundingVolume* _pOther ) const {
    // BoundingBox
    const BoundingBox* pAABB = dynamic_cast<const BoundingBox*>( _pOther );

    if ( pAABB ) {
        return Contains( *pAABB );
    }
    
    // Circle
    const Circle* pCircle = dynamic_cast<const Circle*>( _pOther );

    if ( pCircle ) {
        return Contains( *pCircle );
    }
    
    // Line
    const Line* pLine = dynamic_cast<const Line*>( _pOther );

    if ( pLine ) {
        return Contains( *pLine );
    }

    return false;
}

/*********************** Accessors ************************/
void BoundingVolume::SetCenter( float _fX, float _fY ) {
    m_Center.x = _fX;
    m_Center.y = _fY;
}

void BoundingVolume::MoveCenter( float _fX, float _fY ) {
    m_Center.x += _fX;
    m_Center.y += _fY;
}

void BoundingVolume::SetSize( float _fX, float _fY ) {
    m_Size.x = _fX;
    m_Size.y = _fY;
    m_HalfSize = m_Size * 0.5f;
}

    } // namespace math
} // namespace smeg